#include <math.h>
#include "kinect.h"

Kinect::Kinect()
{
    // Register types used in signal/slot mechanism
    qRegisterMetaType<Kinect::KinectStatus>( "Kinect::KinectStatus" );
    qRegisterMetaType<Kinect::CalibrationStatus>( "Kinect::CalibrationStatus" );
    status=Kinect::Idle;
    displayInfoDepth = true;
    displayInfoImage = true;


}

Kinect::~Kinect()
{
    g_recorder.Release();
    g_recorder2.Release();
    stop();
}
void Kinect::start()
{
    t_requeststop=false;
    status=Kinect::Idle;
    QThread::start();
}

void Kinect::stop()
{
   t_requeststop=true;
   wait();
}
void Kinect::setDisplayInfoDepth(bool draw)
{
   QMutexLocker locker(&mutex);
   Kinect::displayInfoDepth = draw;
}

/**
   \brief Display info about the state of the objects (tracked, ...) on the camera image
**/
void  Kinect::setDisplayInfoImage(bool draw)
{
   QMutexLocker locker(&mutex);
   Kinect::displayInfoImage = draw;
}

unsigned Kinect::getFrameID()
{
   QMutexLocker locker(&mutex);
   return frameid;
}
double Kinect::getTimestamp()
{
    QMutexLocker locker(&mutex);
    return timestamp;
}

QImage Kinect::getDepth()
{
   QMutexLocker locker(&mutex);
   return imageDepth;
}
QImage Kinect::getCamera()
{
   QMutexLocker locker(&mutex);
   return imageCamera;
}

void Kinect::getCameraDepth(QImage &camera,QImage &depth, double &ts, unsigned &fid)
{
   QMutexLocker locker(&mutex);
   camera = imageCamera;
   depth = imageDepth;
   ts = timestamp;
   fid = frameid;
}

QString Kinect::getErrorMsg()
{
   QMutexLocker locker(&mutex);
   return errorMsg;

}

bool Kinect::isRunning()
{
   if(QThread::isRunning())
      return true;
   return false;

}

bool Kinect::isStopped()
{
   if(QThread::isRunning())
      return false;
   return true;

}

void Kinect::run()
{
    mutex.lock();
    status=Kinect::Initializing;
    emit statusNotification(status);
    mutex.unlock();

    bool ok = initialize();

    if(!ok)
    {
       mutex.lock();
       status = Kinect::ErrorStop;
       emit statusNotification(status);
       mutex.unlock();
       return;
    }

    mutex.lock();
    status = Kinect::OkRun;
    emit statusNotification(status);
    mutex.unlock();

    frameid=0;
    while(!t_requeststop)
    {
       //double t1,t2;
       //t1 = PreciseTimer::QueryTimer();
       XnStatus status = g_Context.WaitAndUpdateAll();
       //msleep(100+(rand()%100));   // simulate some shit delay
       //if( (frameid%100) > 50)
         // msleep(((frameid%100)-50)*10);           // simulate some slowing down delay delay
       //t2 = PreciseTimer::QueryTimer();
       //printf("Waitandupdate: %lf. %s\n",(t2-t1)*1000.0,xnGetStatusString(status));

       // Prepare the data to export outside of the thread
       mutex.lock();
       xn::DepthMetaData depthMD;
       g_DepthGenerator.GetMetaData(depthMD);
       //frameid = depthMD.FrameID();
       frameid++;
       //printf("frame id: %d %d\n",frameid,depthMD.FrameID());
       timestamp = (double)depthMD.Timestamp()/1000000.0;
       // Must create the bodies first

       // Then can create the images, which may overlay the bodies
       imageCamera = createCameraImage();
       imageDepth = createDepthImage();
       emit dataNotification();
       mutex.unlock();


    }
    g_Context.Shutdown();

    mutex.lock();
    status = Kinect::Idle;
    emit statusNotification(status);
    mutex.unlock();
}

bool Kinect::initialize()
{
    XnStatus nRetVal = XN_STATUS_OK;
    XnMapOutputMode outputMode;
    XnCallbackHandle hCalibrationStart, hCalibrationComplete;


    // Initialize the context
    nRetVal = g_Context.Init();
    if(nRetVal!=XN_STATUS_OK)
    {
       mutex.lock();
       errorMsg = QString("Context creation failed: %1").arg(xnGetStatusString(nRetVal));
       mutex.unlock();
       return false;
    }

    // Create a depth generator
    nRetVal = g_DepthGenerator.Create(g_Context);
    if(nRetVal!=XN_STATUS_OK)
    {
       mutex.lock();
       errorMsg = QString("Depth generator creation failed: %1").arg(xnGetStatusString(nRetVal));
       mutex.unlock();
       return false;
    }
    outputMode.nXRes = 640;
    outputMode.nYRes = 480;
    outputMode.nFPS = 30;
    nRetVal = g_DepthGenerator.SetMapOutputMode(outputMode);
    if(nRetVal!=XN_STATUS_OK)
    {
       mutex.lock();
       errorMsg = QString("Depth generator SetMapOutputMode failed: %1").arg(xnGetStatusString(nRetVal));
       mutex.unlock();
       return false;
    }

    // Create an image generator
    nRetVal = g_ImageGenerator.Create(g_Context);
    if(nRetVal!=XN_STATUS_OK)
    {
       mutex.lock();
       errorMsg = QString("Image generator creation failed: %1").arg(xnGetStatusString(nRetVal));
       mutex.unlock();
       return false;
    }
    outputMode.nXRes = 640;
    outputMode.nYRes = 480;
    outputMode.nFPS = 30;
    nRetVal = g_ImageGenerator.SetMapOutputMode(outputMode);
    if(nRetVal!=XN_STATUS_OK)
    {
       mutex.lock();
       errorMsg = QString("Image generator SetMapOutputMode failed: %1").arg(xnGetStatusString(nRetVal));
       mutex.unlock();
       return false;
    }

    // Creates a user generator

    if(nRetVal!=XN_STATUS_OK)
    {
       mutex.lock();
       errorMsg = QString("User generator creation failed: %1").arg(xnGetStatusString(nRetVal));
       mutex.unlock();
       return false;
    }



    // Start producting data
    nRetVal = g_Context.StartGeneratingAll();
    nRetVal = g_recorder.Create(g_Context);
    nRetVal = g_recorder2.Create(g_Context);
    // TODO: check error code

    // Init it
    nRetVal = g_recorder.SetDestination(XN_RECORD_MEDIUM_FILE, "C:\\Users\\javier\\VisionRobot1\\release\\rataProfundidad.oni");
    nRetVal = g_recorder2.SetDestination(XN_RECORD_MEDIUM_FILE, "C:\\Users\\javier\\VisionRobot1\\release\\rataVGA.oni");
    // TODO: check error code

    // Add depth node to recording
    nRetVal = g_recorder2.AddNodeToRecording(g_ImageGenerator, XN_CODEC_JPEG);
    nRetVal = g_recorder.AddNodeToRecording(g_DepthGenerator, XN_CODEC_16Z_EMB_TABLES);
    // TODO: check error code
    return true;

}
QImage Kinect::createDepthImage()
{
   // Here must mutex / run also access the data
   xn::SceneMetaData smd;
   xn::DepthMetaData dmd;
   g_DepthGenerator.GetMetaData(dmd);

   XnUInt16 g_nXRes = dmd.XRes();
   XnUInt16 g_nYRes = dmd.YRes();

   QImage image(g_nXRes,g_nYRes,QImage::Format_RGB32);


   const XnDepthPixel* pDepth = dmd.Data();
   const XnLabel* pLabels = smd.Data();

   for (unsigned nY=0; nY<g_nYRes; nY++)
   {
      uchar *imageptr = image.scanLine(nY);

      for (unsigned nX=0; nX < g_nXRes; nX++)
      {
         unsigned depth = *pDepth;
         unsigned label = *pLabels;


         unsigned maxdist=5000;
         if(depth>maxdist) depth=maxdist;
         if(depth)
         {
            depth = (maxdist-depth)*255/maxdist;
         }
         // depth: 0: invalid
         // depth: 255: closest
         // depth: 1: furtherst (maxdist distance)



            // Here we could do depth*color, to show the colored depth
            imageptr[0] = 0;
            imageptr[1] = depth;
            imageptr[2] = 0;
            imageptr[3] = 0xff;
         pDepth++;
         imageptr+=4;
         pLabels++;
      }
   }

   return image;

}
QImage Kinect::createCameraImage()
{

   // Here must mutex / run also access the data
   xn::DepthMetaData dmd;
   xn::ImageMetaData imd;
   g_DepthGenerator.GetMetaData(dmd);
   g_ImageGenerator.GetMetaData(imd);

   XnUInt16 g_nXRes = dmd.XRes();
   XnUInt16 g_nYRes = dmd.YRes();

   QImage image(g_nXRes,g_nYRes,QImage::Format_RGB32);


   const XnUInt8 *idata = imd.Data();
   for (unsigned nY=0; nY<g_nYRes; nY++)
   {
      uchar *imageptr = image.scanLine(nY);

      for (unsigned nX=0; nX < g_nXRes; nX++)
      {
         imageptr[0] = idata[2];
         imageptr[1] = idata[1];
         imageptr[2] = idata[0];
         imageptr[3] = 0xff;

         imageptr+=4;
         idata+=3;

      }
   }
   return image;
}
